#!/usr/bin/env python
# use moveit_commander (the Python MoveIt user interfaces )
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi

class MoveGroupInteface(object):
        def __init__(self):
                super(MoveGroupInteface, self).__init__()
                ######################### setup ############################
                moveit_commander.roscpp_initialize(sys.argv)
                rospy.init_node('ur_move_test_node', anonymous=True)
                self.robot = moveit_commander.RobotCommander()
                self.scene = moveit_commander.PlanningSceneInterface()
                group_name = "manipulator"
                self.move_group_commander = moveit_commander.MoveGroupCommander(group_name)
                self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory,queue_size=20)
                ################ Getting Basic Information ######################
                self.planning_frame = self.move_group_commander.get_planning_frame()
                print("============ Planning frame: %s" % self.planning_frame)
                self.eef_link = self.move_group_commander.get_end_effector_link()
                print("============ End effector link: %s" % self.eef_link)
                self.group_names = self.robot.get_group_names()
                print("============ Available Planning Groups:",self.robot.get_group_names())
                print("============ Printing robot state:=====")
                print(self.robot.get_current_state())
            
        def show(self):
                wpose = self.move_group_commander.get_current_pose().pose
                print(wpose.position.x)
                print(wpose.position.y)
                print(wpose.position.z)
        def plan_cartesian_path(self, x,y,z):
                waypoints = []
                wpose = self.move_group_commander.get_current_pose().pose
                wpose.position.z = wpose.position.z + z*0.01
                if wpose.position.z < 0.35:
                    wpose.position.z = 0.35
                waypoints.append(copy.deepcopy(wpose))
                wpose.position.x = wpose.position.x + x*0.01
                waypoints.append(copy.deepcopy(wpose))
                wpose.position.y = wpose.position.y + y*0.01
                waypoints.append(copy.deepcopy(wpose))
                print("X")
                print(wpose.position.x)
                print("Y")
                print(wpose.position.y)
                print("Z")
                print(wpose.position.z)
                (plan, fraction) = self.move_group_commander.compute_cartesian_path(
										waypoints,   # waypoints to follow
										0.01,      # eef_step  
										0.0)         # jump_threshold  

                # Note: We are just planning, not asking move_group to actually move the robot yet:
                print("=========== Planning completed, Cartesian path is saved=============")
                return plan, fraction
        def execute_plan(self, plan):
                self.move_group_commander.execute(plan, wait=True)
tutorial = MoveGroupInteface()
fo = open("/home/yousongyang/file","r")
while True:
    tutorial.show()
   # do_str = ""
   # while True:
   #     text = fo.readline()
   #     if not text
   #         break
   #     do_str = text
   # if do_str == ""
   #     continue
   # print(do_str,end="")
   # data = do_str.split();
   # print ("name",data[0])
   # print ("X",data[1])
   # print ("Y",data[2])
   # print ("dis",data[3])
   # if input("clu?") != "Y"
   #     continue

    X = (int)(input("X"))
    Y = (int)(input("Y"))
    Z = (int)(input("Z"))
    cartesian_plan, fraction = tutorial.plan_cartesian_path(x=X,y=Y,z=Z)
    if fraction == 1:
        print ("can do enter")
        input()
        tutorial.execute_plan(cartesian_plan)
    else:
        print("can't")
